
#include "controller_simple.h"


namespace legged{

    ControllerSimple::init(){


        // Hardware interface
        auto* hybridJointInterface = robot_hw->get<HybridJointInterface>();
        std::vector<std::string> joint_names{
            "leg_l1_joint", "leg_l2_joint", "leg_l3_joint", "leg_l4_joint", "leg_l5_joint",
            "leg_r1_joint", "leg_r2_joint", "leg_r3_joint", "leg_r4_joint", "leg_r5_joint"
        };
        for (const auto& joint_name : joint_names)
        {
            hybridJointHandles_.push_back(hybridJointInterface->getHandle(joint_name));
        }

    }



}



